#include "../include/force_control_hardware.h"
#define PI 3.1415926

using namespace std;

ForceControlHardware::ForceControlHardware(){
}

ForceControlHardware::~ForceControlHardware(){
}

bool ForceControlHardware::init(Clock::time_point time0, FTInterfaces *ft, RobotInterfaces *robot, RobotSixaxisX *_xbot) {
  _robot = robot;
  _ft = ft;
  _kuka_bot = _xbot;
  return true;
}

int ForceControlHardware::getState(double *pose, double *wrench)
{
  getPose(pose);
  return getWrench(wrench);
}

void ForceControlHardware::getPose(double *pose)
{
  _robot->getCartesian(pose);
}

int ForceControlHardware::getWrench(double *wrench)
{
  double pose[7] = {0};
  _robot->getCartesian(pose);
  int safety_flag = _ft->getWrenchNetTool(pose, wrench);

  return safety_flag;
}

bool ForceControlHardware::checkPose(const double *pose_set){
  double pose_fb[7];
  getPose(pose_fb);
  for (auto i = 0; i < 3; i++){
    if(fabs(pose_set[i]-pose_fb[i]) > _robot->_check_zone[i])
      return false;
  }
  return true; 
}

void ForceControlHardware::setPose(const double *pose_set)
{
  /*  transform tool 2 world  into tool 2 base  */
  double *_joint = new double[6](); 
  Matrix4 _w2b = posem2SE3(_robot->getWorld2Base());

  Matrix4 _t2w = posem2SE3(pose_set);
  Matrix4 _t2b = _w2b * _t2w;

  Frame _pose = Frame::FromMatrix(_t2b);//in base frame
  AxisArrayX cal_q = _kuka_bot->IkinPos(_pose, Redundancy(2,0));

  for(auto i=1; i<6; i++){
    _joint[i]=cal_q[i];
  }
  _joint[0]=-cal_q[0];
  _robot->setJoints(_joint);
}

void ForceControlHardware::init_param(){
  copyArray(_robot->_vrep_pose_T2S, _ft->PoseTool2Sensor, 7);
  _ft->_adj_tool_2_sensor = dynamica::SE32Adj(dynamica::posem2SE3(_ft->PoseTool2Sensor));
}


